{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "05124c35",
   "metadata": {
    "colab_type": "text",
    "id": "EgiF12Hf1Dhs"
   },
   "source": [
    "This notebook provides examples to go along with the [textbook](http://manipulation.csail.mit.edu/robot.html).  I recommend having both windows open, side-by-side!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "99268d0c",
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "eeMrMI0-1Dhu",
    "lines_to_end_of_cell_marker": 2
   },
   "outputs": [],
   "source": [
    "from pydrake.all import ModelVisualizer, StartMeshcat, PackageMap, Simulator\n",
    "\n",
    "from manipulation import running_as_notebook, ConfigureParser, FindResource\n",
    "from manipulation.station import load_scenario, MakeHardwareStation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7dec0a5e",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Start the visualizer.\n",
    "meshcat = StartMeshcat()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def AddSpotRemote(parser):\n",
    "    parser.package_map().AddRemote(\n",
    "        package_name=\"spot_description\",\n",
    "        params=PackageMap.RemoteParams(\n",
    "            urls=[\n",
    "                f\"https://github.com/bdaiinstitute/spot_ros2/archive/d429947a1df842ec38f8c6099dde9501945090d6.tar.gz\"\n",
    "            ],\n",
    "            sha256=(\n",
    "                \"e4dd471be4e7e822a12afcfd6a94ce7ecbb39e2d4ea406779a96e146a607bf53\"\n",
    "            ),\n",
    "            strip_prefix=\"spot_ros2-d429947a1df842ec38f8c6099dde9501945090d6/spot_description/\",\n",
    "        ),\n",
    "    )"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7112a8be",
   "metadata": {},
   "source": [
    "# Simplified Spot model for mobile manipulation\n",
    "\n",
    "First we'll use the ModelVisualizer to inspect the model."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ec925772",
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "5SjOClhTltPk"
   },
   "outputs": [],
   "source": [
    "visualizer = ModelVisualizer(meshcat=meshcat)\n",
    "ConfigureParser(visualizer.parser())\n",
    "AddSpotRemote(visualizer.parser())\n",
    "visualizer.AddModels(\n",
    "    url=\"package://manipulation/spot/spot_with_arm_and_floating_base_actuators.urdf\"\n",
    ")\n",
    "visualizer.Run(loop_once=not running_as_notebook)\n",
    "meshcat.DeleteAddedControls()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now we can use HardwareStation to create a basic simulation."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cfaf4640",
   "metadata": {},
   "outputs": [],
   "source": [
    "scenario = load_scenario(\n",
    "    filename=FindResource(\n",
    "        \"models/spot/spot_with_arm_and_floating_base_actuators.scenario.yaml\"\n",
    "    )\n",
    ")\n",
    "station = MakeHardwareStation(\n",
    "    scenario, meshcat, parser_preload_callback=AddSpotRemote\n",
    ")\n",
    "simulator = Simulator(station)\n",
    "context = simulator.get_mutable_context()\n",
    "x0 = station.GetOutputPort(\"spot.state_estimated\").Eval(context)\n",
    "station.GetInputPort(\"spot.desired_state\").FixValue(context, x0)\n",
    "simulator.AdvanceTo(0.1);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3.10.8 64-bit",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
